import numpy as np
import control
import matplotlib.pyplot as plt

# --- 被控对象 G(s) = 10 / (s^2 + 3s + 10)
numG = [10]
denG = [1, 3, 10]
G = control.tf(numG, denG)

# --- PID 参数（初值，来自极点配置）
Kp = 5.637261079605127
Ki = 12.176226526518303
Kd = 1.5492753623188407
N  = 15.0  # 微分滤波系数（建议 10~20）

# C(s) = Kp + Ki/s + Kd*s/(1 + s/N)
C = control.tf([Kd*N, Kp*N, Ki], [1, N])  # 等价实现：把 D 滤波并联并通分

# 闭环（单位反馈）
T = control.feedback(C*G, 1)

# 仿真
t = np.linspace(0, 5, 800)
t, y = control.step_response(T, T=t)

# 指标简算（粗略）
yss = y[-1]
Mp = (y.max()-yss)/yss * 100
# 2%稳态时间
eps = 0.02*abs(yss)
settling = t[np.where(np.abs(y - yss) <= eps)[0]]
Ts = settling[0] if len(settling) else np.nan

print(f"Steady-state ~ {yss:.3f},  Overshoot ~ {Mp:.1f}% ,  Ts(2%) ~ {Ts:.2f}s")

plt.figure()
plt.plot(t, y, label='Closed-loop step')
plt.axhline(yss, linestyle='--', linewidth=1)
plt.xlabel('t [s]'); plt.ylabel('y'); plt.grid(True); plt.legend()
plt.title('PID closed-loop step response')
plt.show()